#pragma once

#include <g2o/stuff/macros.h>
#include <g2o/core/factory.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/linear_solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>

#include "graph_optimizer/edge/edge_gnss.hpp"
#include "graph_optimizer/edge/edge_og.hpp"

G2O_USE_OPTIMIZATION_LIBRARY(pcg)
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
G2O_USE_OPTIMIZATION_LIBRARY(csparse)

namespace graph_optimizer
{
  class Optimizer
  {
  public:
    Optimizer(const std::string &solver_type = "lm_var");
    // 优化
    bool Optimize();
    // 输出数据
    bool GetOptimizedPose(std::deque<PoseData> &optimized_pose);
    int GetNodeNum();
    // 添加节点、边、鲁棒核
    void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size);
    void AddNode(const PoseData &pose);
    void AddOgEdge(int vertex_index1,
                   int vertex_index2,
                   const std::shared_ptr<PreIntegration> &pre_integration);
    void AddGnssEdge(int gnss_vertex_index,
                     const Eigen::Vector3d &xyz);
    void SetMaxIterationsNum(int max_iterations_num);

  private:
    void AddRobustKernel(g2o::OptimizableGraph::Edge *edge, const std::string &kernel_type, double kernel_size);

    int max_iterations_num_ = 512;
    g2o::RobustKernelFactory *robust_kernel_factory_;
    std::unique_ptr<g2o::SparseOptimizer> graph_ptr_;

    std::string robust_kernel_name_;
    double robust_kernel_size_;
    bool need_robust_kernel_ = false;
    std::deque<double> stamps;
  };
} // namespace graph_optimizer
